package tazadum.engine.math.curve;

import java.util.Collection;
import java.util.Map;
import java.util.TreeMap;

import tazadum.engine.math.Vector;

public class BSplineCurve implements Curve {
	protected Map<Integer, Vector> map = new TreeMap<Integer, Vector>();

	@Override
	public void reset() {
		map.clear();
	}

	@Override
	public void add(final Vector node) {
		map.put(map.size(), node);
	}

	@Override
	public void add(final Vector... nodes) {
		for (final Vector node : nodes) {
			map.put(map.size(), node);
		}
	}

	public int nodeCount() {
		return map.size();
	}

	public Collection<Vector> nodes() {
		return map.values();
	}

	public Vector[] calculate(Vector[] destination, final float t0, final float t1, final int steps) {
		if (destination == null) {
			destination = new Vector[steps];
		}
		final int size = map.size();
		if (size < 4) {
			return destination;
		}

		final float start = t0 >= 0 ? t0 : 0;
		final float end = t1 >= size ? size : t1;
		final double stepSize = (end - start) / (double) steps;

		for (int i = 0; i < steps; i++) {
			final float time = (float) (i * stepSize + start);

			final float adjusted = (time < 0) ? 1 : time + 1;
			final int index = (adjusted < size - 3) ? (int) adjusted : (size - 3);

			final Vector a = map.get(index - 1);
			final Vector b = map.get(index);
			final Vector c = map.get(index + 1);
			final Vector d = map.get(index + 2);

			final float alpha = (adjusted >= size - 3) ? (adjusted - index) * 0.25f : (adjusted - index);
			final float x = interpolate(alpha, a.x, b.x, c.x, d.x);
			final float y = interpolate(alpha, a.y, b.y, c.y, d.y);
			final float z = interpolate(alpha, a.z, b.z, c.z, d.z);
			destination[i] = new Vector(x, y, z);

		}

		return destination;
	}

	protected final float interpolate(final float t, final float A, final float B, final float C, final float D) {
		final float a0 = (-1 * A + 3 * B - 3 * C + D) / 6.0f;
		final float a1 = (+3 * A - 6 * B + 3 * C) / 6.0f;
		final float a2 = (-3 * A + 0 * B + 3 * C) / 6.0f;
		final float a3 = (+1 * A + 4 * B + 1 * C) / 6.0f;
		return a3 + t * (a2 + t * (a1 + t * a0));
	}

	@Override
	public Vector calculate(final float time) {
		final int size = map.size();
		if (size < 4) {
			return null;
		}
		final float adjusted = (time < 0) ? 1 : time + 1;
		final int index = (adjusted < size - 3) ? (int) adjusted : (size - 3);

		final Vector a = map.get(index - 1);
		final Vector b = map.get(index + 0);
		final Vector c = map.get(index + 1);
		final Vector d = map.get(index + 2);

		final float alpha = (adjusted >= size - 3) ? (adjusted - index) * 0.25f : (adjusted - index);
		final float x = interpolate(alpha, a.x, b.x, c.x, d.x);
		final float y = interpolate(alpha, a.y, b.y, c.y, d.y);
		final float z = interpolate(alpha, a.z, b.z, c.z, d.z);
		return new Vector(x, y, z);
	}
}
